#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <sensor_msgs/point_cloud2_iterator.hpp>
#include <std_msgs/msg/float32_multi_array.hpp>
#include <cmath>

class Ros2Ros : public rclcpp::Node
{
public:
    Ros2Ros() : Node("ros2ros")
    {
        sub_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant);
        rclcpp::SubscriptionOptions options_sub;
        options_sub.callback_group = sub_callback_group_;

        pub_callback_group_ = create_callback_group(rclcpp::CallbackGroupType::Reentrant);

        subscription_ = this->create_subscription<sensor_msgs::msg::PointCloud2>(
            "/line_point", 10,
            std::bind(&Ros2Ros::pointCloudCallback,
                      this, std::placeholders::_1),
            options_sub);

        to_do_ = this->create_publisher<std_msgs::msg::Float32MultiArray>("float_array_topic", 5);
    }

private:
    int valve_state = 0;
    int state_first = 0;
    int state_second = 0;

    rclcpp::CallbackGroup::SharedPtr sub_callback_group_;
    rclcpp::CallbackGroup::SharedPtr pub_callback_group_;

    void pointCloudCallback(const sensor_msgs::msg::PointCloud2::SharedPtr msg)
    {
        /* ---------- 1. 先清表 ---------- */
        std::unordered_map<float, float> left_min_y;   // x -> 最小 y
        std::unordered_map<float, float> right_min_y;

        /* ---------- 2. 老变量照用 ---------- */
        float z_high  = 0.0f;
        int   total   = 0;
        static rclcpp::Rate rate(10);
        rate.sleep();

        sensor_msgs::PointCloud2Iterator<float> iter_x(*msg, "x");
        sensor_msgs::PointCloud2Iterator<float> iter_y(*msg, "y");
        sensor_msgs::PointCloud2Iterator<float> iter_z(*msg, "z");
        sensor_msgs::PointCloud2Iterator<float> iter_i(*msg, "intensity");

        /* ---------- 3. 循环里只加两行 ---------- */
        for (; iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_z, ++iter_i)
        {
            if (*iter_y < 0.6f || *iter_y > 29.0f) continue;

            std::cout << "来自ROS2的消息\n";
            RCLCPP_INFO(this->get_logger(),
                        "Point: x=%f  y=%f  z=%f  I=%f",
                        *iter_x, *iter_y, *iter_z, *iter_i);

            z_high += *iter_z; ++total;

            if (std::abs(*iter_i - 250.0f) < 0.5f)
            {
                float &ymin = left_min_y[*iter_x];          // 同 x 的最小 y
                if (ymin == 0.0f) ymin = std::numeric_limits<float>::max();
                if (*iter_y < ymin) ymin = *iter_y;
            }
            if (std::abs(*iter_i - 1500.0f) < 0.5f)
            {
                float &ymin = right_min_y[*iter_x];
                if (ymin == 0.0f) ymin = std::numeric_limits<float>::max();
                if (*iter_y < ymin) ymin = *iter_y;
            }
        }

        /* ---------- 4. 找全局最小 (x, y_min) ---------- */
        float left_nearest_y  =  std::numeric_limits<float>::max();
        float left_nearest_x  =  0.0f;
        for (const auto &kv : left_min_y)
        {
            if (kv.second < left_nearest_y)
            {
                left_nearest_y  = kv.second;
                left_nearest_x  = kv.first;
            }
        }

        float right_nearest_y  =  std::numeric_limits<float>::max();
        float right_nearest_x  =  0.0f;
        for (const auto &kv : right_min_y)
        {
            if (kv.second < right_nearest_y)
            {
                right_nearest_y  = kv.second;
                right_nearest_x  = kv.first;
            }
        }

        /* ---------- 5. 发出去 ---------- */
        std_msgs::msg::Float32MultiArray merged_pose;
        merged_pose.data.resize(3);   // 扩展成 6 个
        merged_pose.data[0] = right_nearest_x * 1000;
        // merged_pose.data[1] = right_nearest_y;
        merged_pose.data[1] = left_nearest_x * 1000;
        // merged_pose.data[3] = left_nearest_y;
        // merged_pose.data[3] = (total > 0) ? z_high / total : 0.0f;
        merged_pose.data[2] = 2170.0f;

        // merged_pose.data[5] = 0.0f;   // 预留
        to_do_->publish(merged_pose);

        RCLCPP_INFO(this->get_logger(),
                    "nearest->  right(x=%.2f y=%.2f)  left(x=%.2f y=%.2f)",
                    right_nearest_x* 1000, right_nearest_y* 1000,
                    left_nearest_x* 1000,  left_nearest_y* 1000);
    }

    rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr subscription_;
    rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr to_do_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<Ros2Ros>();
    rclcpp::executors::MultiThreadedExecutor executor;
    executor.add_node(node);
    executor.spin();
    rclcpp::shutdown();
    return 0;
}